#!/usr/bin/python3
from xgolib import XGO  
from geometry_msgs.msg import Twist
import math
import time


if __name__ == '__main__':
    dog = XGO("xgolite")
    dog.move('x', 0)
    dog.move('y', 0)
    # dog.turn(0)

    #抓取

    dog.claw(0)
    dog.arm(110, -95)
    dog.attitude(['p','y','r'],[20,0, 0])
    time.sleep(3)
    dog.claw(255)
    time.sleep(1)
    dog.arm(90, 95)
    dog.attitude(['p','y','r'],[0,0, 0])
    time.sleep(5)
    #放下
    dog.arm(110, -95)
    dog.attitude(['p','y','r'],[20,0, 0])
    time.sleep(2)
    dog.claw(0)
    time.sleep(2)
    dog.attitude(['p','y','r'],[0,0, 0])
    dog.reset()

